#include "FindArmor.h"

/***********************************
 * 函数声明：FindTarget
 * 外部调用函数
 * 形式参数说明：
 * lights: 识别出的灯条
 * target：目标（包括yaw、pitch、distance三个float类型的数据）
 *  
 **********************************/
void FindArmor::FindTarget(const std::vector<cv::RotatedRect> &lights,cv::Point3f &target){
    /*请在以下区域编写程序*/
}


void FindArmor::SolveTarget(const cv::Rect &final_armor,cv::Point3f &target){
    static float half_camera_width = 320;
    static float half_camera_height = 240;
    static float camera_fx = 1300;
    static float camera_fy = 1300;
    
    cv::Point2f armor_center = cv::Point2f(final_armor.x+final_armor.width/2.0,final_armor.y+final_armor.height/2.0);
    float yaw_offset = atan2(half_camera_width - armor_center.x, camera_fx) * rad;
    float pitch_offset = atan2(half_camera_height - armor_center.y, camera_fy) * rad;
    float distance=camera_fx * 125 / final_armor.height / 1000;
}